#include "motor.h"
#include "PWM.h"

/**
 * @brief TB6612初始化
 */
void Motor_Init(void)
{
    GPIO_InitTypeDef GPIO_InitStructure;

    RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB, ENABLE);
    GPIO_InitStructure.GPIO_Mode = GPIO_Mode_Out_PP;    // 初始化GPIO--PB12、PB13、PB14、PB15为推挽输出
    GPIO_InitStructure.GPIO_Pin = GPIO_Pin_12 | GPIO_Pin_13 | GPIO_Pin_14 | GPIO_Pin_15;
    GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
    GPIO_Init(GPIOB, &GPIO_InitStructure);
}

/**
 * @brief 限幅函数
 */
void Limit(int32_t *motoA, int32_t *motoB)
{
    if(*motoA>PWM_MAX) *motoA = PWM_MAX;
    if(*motoA<PWM_MIN) *motoA = PWM_MIN;

    if(*motoB>PWM_MAX) *motoB = PWM_MAX;
    if(*motoB<PWM_MIN) *motoB = PWM_MIN;
}

/**
 * @绝对值函数
 */
uint16_t GFP_abs(int16_t p)
{
    uint16_t q;
    q = p>0 ? p : (-p);
    return q;
}

/**
 * @brief 赋值函数
 * @param PWM value
 */
void Motor_Load(int16_t moto1, int16_t moto2)
{
    // 1. 研究正负号， 对应正反转
    if (moto1 > 0) {
        // GPIO_SetBits(GPIOB, GPIO_Pin_13);
        // GPIO_ResetBits(GPIOB, GPIO_Pin_12);
        // PWM_SetCompare1(GFP_abs(moto1));
        
        GPIO_ResetBits(GPIOB, GPIO_Pin_13);
        GPIO_ResetBits(GPIOB, GPIO_Pin_12);

    } else {
        // GPIO_SetBits(GPIOB, GPIO_Pin_12);
        // GPIO_ResetBits(GPIOB, GPIO_Pin_13);
        // PWM_SetCompare1(GFP_abs(moto1)); 

        GPIO_ResetBits(GPIOB, GPIO_Pin_13);
        GPIO_ResetBits(GPIOB, GPIO_Pin_12);         
    }

    if (moto2 > 0) {
        GPIO_SetBits(GPIOB, GPIO_Pin_14);
        GPIO_ResetBits(GPIOB, GPIO_Pin_15);
        PWM_SetCompare4(GFP_abs(moto2));
    } else {
        GPIO_SetBits(GPIOB, GPIO_Pin_15);
        GPIO_ResetBits(GPIOB, GPIO_Pin_14);
        PWM_SetCompare4(GFP_abs(moto2));   
    }
}


// void moto(int16_t mode)
// {
//     if (mode == 1) {
//         GPIO_SetBits(GPIOB, GPIO_Pin_15);
//         GPIO_ResetBits(GPIOB, GPIO_Pin_14);

//         GPIO_ResetBits(GPIOB, GPIO_Pin_13);
//         GPIO_SetBits(GPIOB, GPIO_Pin_12);
//     }
//     if (mode == 0) {
//         GPIO_ResetBits(GPIOB, GPIO_Pin_15);
//         GPIO_SetBits(GPIOB, GPIO_Pin_14);

//         GPIO_SetBits(GPIOB, GPIO_Pin_13);
//         GPIO_ResetBits(GPIOB, GPIO_Pin_12);
//     }
// }
